#include "collisionstrategy.h"

//sphere or AABB or OBB
CollisionStrategy::CollisionStrategy(std::function<void(OctreeNode* ,const Cuboid&)> CollisionOctreeNodeStrategy,\
                                     std::function<bool(const std::vector<OctreeNode*>& collisionnode, const Cuboid& robot)> CollisionPointStrategy):\
m_CollisionOctreeNodeStrategy(CollisionOctreeNodeStrategy),\
m_CollisionPointStrategy(CollisionPointStrategy),\
m_nodeckecktime(0.f),m_pointchecktime(0.f)
{};

CollisionStrategy::CollisionStrategy(std::function<void(OctreeNode* ,const Cuboid&,std::vector<OctreeNode *> &)> CollisionOctreeNodeStrategy,\
                  std::function<bool(const std::vector<OctreeNode*>& collisionnode, const Cuboid& robot,std::unordered_set<Point*>&)> CollisionPointStrategy):
    m_CollisionOctreeNodeStrategyThread(CollisionOctreeNodeStrategy),\
    m_CollisionPointStrategyThread(CollisionPointStrategy),\
    m_nodeckecktime(0.f),m_pointchecktime(0.f)
    {};


void CollisionStrategy::calculateToOnePart(OctreeNode* node,const Cuboid& robot,std::vector<OctreeNode*>& collisionnode,std::unordered_set<Point*>& results){
    collisionnode.clear();

    CELLTimestamp timernode;
    timernode.update();
    m_CollisionOctreeNodeStrategyThread(node,robot,collisionnode);
    m_nodeckecktime += timernode.getElapsedTimeInMilliSec();

    CELLTimestamp timerpoint;
    timerpoint.update();
    m_CollisionPointStrategyThread(collisionnode,robot,results);
    m_pointchecktime += timerpoint.getElapsedTimeInMilliSec();

}
bool CollisionStrategy::calculateToOnePart(OctreeNode* node,const Cuboid& robot){
    collisionnode.clear();
    //CELLTimestamp timernode;
    //timernode.update();
    m_CollisionOctreeNodeStrategy(node,robot);
    //m_nodeckecktime += timernode.getElapsedTimeInMilliSec();

    //CELLTimestamp timerpoint;
    //timerpoint.update();
    return m_CollisionPointStrategy(collisionnode,robot);
    //m_pointchecktime += timerpoint.getElapsedTimeInMilliSec();
}
bool CollisionStrategy::calculateToParts(OctreeNode* node,const std::vector<Cuboid>& robotpart){
    collisionnode.clear();
    for(const auto& part : robotpart){
        if(calculateToOnePart(node,part))
            return true;
        else
        collisionnode.clear();
    }
    return false;
}

void CollisionStrategy::printResult(){
    std::cout<<"THREAD ---------------------------------------THREAD"<<std::endl;
    std::cout << "Total points where collisions occurred : " << count_for_collisionpoint << std::endl;
//    if(count == 0)
//        std::cout<<"\n      No collision!"<<std::endl;
//    else
//        std::cout << "           Their coordinates"<<std::endl;
//    for (auto point : results)
//    {
//        std::cout << "     point_x = " << point->m_x <<"   "<<"point_y = " << point->m_y <<"   "<<"point_z = " << point->m_z <<std::endl;
//    }
    std::cout << "\nNodeCollision consumes time  = " << m_nodeckecktime << "ms" << std::endl;
    std::cout << "PointCollision consumes time = " << m_pointchecktime << "ms" << std::endl;
    std::cout << "Total consumes time*100      = " << (m_nodeckecktime + m_pointchecktime)*100<< "ms" << std::endl;
    std::cout<<"\nTHREAD ---------------------------------------THREAD"<<std::endl;
}

void CollisionStrategy::clearResult(){
    results.clear();
    count_for_collisionpoint = 0;
    m_nodeckecktime = 0.f;
    m_pointchecktime= 0.f;
}
